#ifndef COLLISION_DETECTOR_H
#define COLLISION_DETECTOR_H

#include "Point.h"
#include "planner_typedef.h"
#pragma comment(lib,"PQP.lib")
#include "PQP.h"
#include "Polygon_triangulation.h"


// PQP
typedef PQP_REAL*				PQP_point;
typedef vector<PQP_REAL*>		PQP_triangle; 
typedef	vector<PQP_triangle>	Mesh;

typedef struct {
    double m[3][3];
} Matrix;

#define PI 3.14159265

class Collision_detector
{
public:
	Collision_detector(vector<Polygon>* obstacles, Polygon robot, double step_size,double step_angle_szie,int bound):
	  m_obstacle_polygons(obstacles), m_robot(robot), m_step_size(step_size), m_step_angle_szie(step_angle_szie),m_bound(bound)
	{
		// add walls to the workspace
		add_walls(obstacles);

		polygon_to_mesh(m_robot, &m_robot_mesh);

		foreach(Polygon poly, *m_obstacle_polygons)
		{
			polygon_to_mesh(poly, &m_obstacle_mesh);
		}

		m_robot_model = mesh_to_model(&m_robot_mesh);
		m_obstacles_model = mesh_to_model(&m_obstacle_mesh);

		IR.m[0][0] = IR.m[1][1] = IR.m[2][2] = 1.0;
		IR.m[0][1] = IR.m[1][0] = IR.m[2][0] = 0.0;
		IR.m[0][2] = IR.m[1][2] = IR.m[2][1] = 0.0;

		IT[0] = IT[1] = IT[2] = 0.0;
	}

	// returns true if configuration p is valid
	bool		valid(Point p)
	{
		set_translation(p);
		set_rotation(p);

		PQP_CollideResult res;
		PQP_Collide(&res, IR.m, IT, m_obstacles_model,
			rm.m, translation, m_robot_model, PQP_FIRST_CONTACT);

		bool colliding = (res.Colliding() == 1);

		return !colliding;
	}

	// returns true if the straight-line path from s to t is collision free
	bool valid(Point s, Point t)
	{
		vector<Point> interpolation_points;
		interpolate(s,t, &interpolation_points);

		for (int i = 1; i < interpolation_points.size(); i++)
		{
			if (!valid(interpolation_points[i]))
				return false;
		}

		return true;
	}

	/*
		Tries to move the robot along a local path
		until reaching an obstacle. Returns the last
		valid configuration.
	*/
	/*Point	steer(Point s, Point t)
	{
		vector<Point> interpolation_points;
		interpolate(s,t, &interpolation_points);
		if (interpolation_points.empty())
			return s;
		for (int i = 1; i < interpolation_points.size(); i++)
		{
			if (!valid(interpolation_points[i]))
				return interpolation_points[i-1];
		}

		return t;
	}*/
	Point steer(Point s, Point t)
	{
		vector<Point> interpolation_points;
		interpolate(s,t, &interpolation_points);
		Point ans;

		double angle = 0;
		if (interpolation_points.empty())
		{
			return s;
		}
		int i;
		int num_steps = interpolation_points.size()<m_bound?interpolation_points.size():m_bound;

		for (i = 1; i <  num_steps; i++)
		{
			if (!valid(interpolation_points[i]))
			{
				i = i>30 ? i-30 : 0;	// the cution side form the wall
				return interpolation_points[i];
			}

		}			

		Point p = interpolation_points[i-1];  
		if(i == interpolation_points.size())
		{
			angle = angle_of_line(s,t);
			p = Point(t[0],t[1],angle);
		}
		return p;
	}

private:

	void	polygon_to_mesh(Polygon poly, Mesh* mesh)
	{
		
		vector<vector<Point>> triangulation = triangulate(poly);
		foreach(vector<Point> tri, triangulation)
		{
			PQP_triangle pqp_triangle;
			foreach(Point p, tri)
			{
				PQP_REAL *pqp_point = new PQP_REAL[3]; 
				pqp_point[0] = p[0]; 
				pqp_point[1] = p[1]; 
				pqp_point[2] = 0.0;
				pqp_triangle.push_back(pqp_point);
			}
			mesh->push_back(pqp_triangle);
		}
	}

	/*
	void		stretch_mesh(Mesh* mesh)
	{
		Mesh temp_mesh;

		foreach(PQP_triangle tri, *mesh)
		{
			PQP_REAL *_0=tri[0];
			PQP_REAL *_1=tri[1];
			PQP_REAL *_2=tri[2];

			make_wall(&temp_mesh, _0, _1);
			make_wall(&temp_mesh, _1, _2);
			make_wall(&temp_mesh, _2, _0);
		}

		mesh->insert(mesh->end(), temp_mesh.begin(), temp_mesh.end());
	}*/

	/*
	void		make_wall(Mesh* mesh, PQP_REAL *p, PQP_REAL *q)
	{
		PQP_REAL *p_up = new PQP_REAL[3];
		PQP_REAL *p_dw = new PQP_REAL[3];
		PQP_REAL *q_up = new PQP_REAL[3];
		PQP_REAL *q_dw = new PQP_REAL[3];
		
		p_up[0] = p[0]; p_up[1] = p[1]; p_up[2] = 1.0; 
		p_dw[0] = p[0]; p_dw[1] = p[1]; p_dw[2] = -1.0;

		q_up[0] = q[0]; q_up[1] = q[1]; q_up[2] = 1.0; 
		q_dw[0] = q[0]; q_dw[1] = q[1]; q_dw[2] = -1.0;

		PQP_triangle tri1, tri2;
		tri1.push_back(p_up); tri1.push_back(q_up); tri1.push_back(q_dw);
		tri2.push_back(p_up); tri2.push_back(p_dw); tri2.push_back(q_dw);

		mesh->push_back(tri1); mesh->push_back(tri2);
	}*/

	PQP_Model*	mesh_to_model(Mesh* mesh)
	{
		PQP_Model* model = new PQP_Model;
		model->BeginModel();

		int id = 0;

		foreach(PQP_triangle t, *mesh)
		{
			model->AddTri(t[0], t[1], t[2], id++);
		}

		model->EndModel();
		return model;
	}

	// sets the translation vector given p
	void	set_translation(Point p)
	{
		translation[0] = p[0];
		translation[1] = p[1];
		translation[2] = 0.0;
	}

	// sets the rotation matrix given p
	void	set_rotation(Point p)
	{
		double ct = cos(p[2] * 360 * PI / 180.0);
		double st = sin(p[2] * 360 * PI / 180.0);
		
		// regular
		rm.m[0][0] = ct; rm.m[0][1] = -st; rm.m[0][2] = 0;
		rm.m[1][0] = st; rm.m[1][1] = ct; rm.m[1][2] = 0;
		rm.m[2][0] = 0; rm.m[2][1] = 0; rm.m[2][2] = 1;
	}


	/*  generates multiple interpolation points between s and t.
		the amount of points is set by the constant m_step_size. */

	void interpolate(const Point s, const Point t,
								vector<Point> *container)
	{
		int num_steps = distance_points(s,t) / m_step_size;

		double angle = angle_of_line(s,t);

		angle = angle - s[2];

		int num_steps_angle = abs(angle / m_step_angle_szie);

		Point base = Point(s);

		//remain in the orignal angle
		Point step((t[0]-s[0]) / num_steps , (t[1]-s[1]) / num_steps ,angle / num_steps_angle);
		
		for (int i = 0; i < num_steps_angle; i++)
		{
			container->push_back(base);
			base=Point(base[0] , base[1] ,base[2]+step[2]);
		}
		
		for (int i = 0; i < num_steps; i++)
		{
				container->push_back(base);
				base=Point(base[0] + step[0],base[1] + step[1],base[2]);
		}
		container->push_back(Point(t[0],t[1],angle));
	}


	// generates wall polygons around the bounding box [-1,1]^2
	void	add_walls(vector<Polygon>* obstacles)
	{
		Polygon poly;
		Point _0,_1,_2,_3;

		_0 = Point(1,1,0), _1 = Point(1,-1,0), _2 = Point(1.1,-1,0), _3 = Point(1.1,1,0);
		poly.push_back(_0); poly.push_back(_1); poly.push_back(_2); poly.push_back(_3);
		obstacles->push_back(poly);
		poly.clear();

		_0 = Point(1,1,0), _1 = Point(-1,1,0), _2 = Point(-1,1.1,0), _3 = Point(1,1.1,0);
		poly.push_back(_0); poly.push_back(_1); poly.push_back(_2); poly.push_back(_3);
		obstacles->push_back(poly);
		poly.clear();

		_0 = Point(-1,1,0), _1 = Point(-1,-1,0), _2 = Point(-1.1,-1,0), _3 = Point(-1.1,1,0);
		poly.push_back(_0); poly.push_back(_1); poly.push_back(_2); poly.push_back(_3);
		obstacles->push_back(poly);
		poly.clear();

		_0 = Point(-1,-1,0), _1 = Point(-1,-1.1,0), _2 = Point(1,-1.1,0), _3 = Point(1,-1,0);
		poly.push_back(_0); poly.push_back(_1); poly.push_back(_2); poly.push_back(_3);
		obstacles->push_back(poly);
		poly.clear();
	}

	double			m_step_size;
	double			m_step_angle_szie;
	vector<Polygon>	*m_obstacle_polygons;
	Polygon			m_robot;
	
	int				m_bound;

	Mesh			m_obstacle_mesh;
	Mesh			m_robot_mesh;

	PQP_Model*		m_robot_model;
	PQP_Model*		m_obstacles_model;

	PQP_CollideResult	m_collision_res;

	Matrix		IR; // identity rotation
	PQP_REAL	IT[3]; // identity translation
	Matrix		rm; // rotation matrix
	 
	PQP_REAL translation[3];
	double	m_accuracy;
};

#endif // COLLISION_DETECTOR_H